#include "Predictor.hpp"
#include "Pose/AngleSolver.hpp"
#include "opencv2/opencv.hpp"
#include <algorithm>
#include "../General/General.h"
#include <iostream>
#include <math.h>
using namespace std;
using namespace cv;

namespace rm
{
void Predictor::refresh()
{
    odometrys.clear();

    move_module_r = STAYSTILL;
    move_module_t = STAYSTILL;
    rotate_module = STAYSTILL;

    delta_pitch_predict = 0;
    delta_yaw_predict = 0;
    delta_pitch = 0;
    delta_yaw = 0;

    sentry_predict_done = false;
}

void Predictor::push(float time, float self_gimbal_pitch, float self_gimbal_yaw, int enemy_num, float enemy_distance, cv::Vec2f enemy_angle, cv::Point3f enemy_direction)
{
    Odometry odometry;
    odometry.time = time/1000;
    odometry.self_gimbal_pitch = self_gimbal_pitch;
    odometry.self_gimbal_yaw = self_gimbal_yaw;
    odometry.enemy_num = enemy_num;
    odometry.enemy_distance = enemy_distance/1000;
    odometry.enemy_angle = enemy_angle;
    odometry.enemy_direction = enemy_direction;

    if(odometrys.size() == 0)
    {
        odometrys.push_back(odometry);
        return;
    }

    Odometry last_odometry = odometrys.at(odometrys.size() - 1);
    if(enemy_num != last_odometry.enemy_num)
    {
        refresh();
        odometrys.push_back(odometry);
        return;
    }

    if(abs(enemy_angle[0] - last_odometry.enemy_angle[0]) > max_change_angle)
    {
        refresh();
        odometrys.push_back(odometry);
        return;
    }

    if(abs(self_gimbal_yaw - last_odometry.self_gimbal_yaw) < 180)
    {
        if(abs(self_gimbal_yaw - last_odometry.self_gimbal_yaw) > max_change_angle)
        {
            refresh();
            odometrys.push_back(odometry);
            //cout<<1<<endl;
            return;
        }
    }
    else
    {
        if(360-abs(self_gimbal_yaw - last_odometry.self_gimbal_yaw) > max_change_angle)
        {
            refresh();
            odometrys.push_back(odometry);
            //cout<<2<<endl;
            return;
        }
    }

    if(time/1000 - last_odometry.time > max_change_time)
    {
        refresh();
        odometrys.push_back(odometry);
        //cout<<3<<endl;
        return;
    }

    if(enemy_distance/1000 - last_odometry.enemy_distance > max_change_distance)
    {
        refresh();
        odometrys.push_back(odometry);
        //cout<<4<<endl;
        return;
    }

    double delta_angle = 180/CV_PI*(acos(odometry.enemy_direction.x) - acos(last_odometry.enemy_direction.x));
    if(abs(delta_angle)>15)
    {
        //cout<<5<<endl;
        return;
    }

    if(odometrys.size() > queue_size)
    {
        refresh();
        odometrys.push_back(odometry);
        //cout<<5<<endl;
        return;
    }

    if(odometrys.size() < queue_size)
    {
        odometrys.push_back(odometry);
        return;
    }

    if(odometrys.size() == queue_size)
    {
        for(int i = 0; i < queue_size - 1; i++)
        {
            odometrys.at(i) = odometrys.at(i + 1);
        }
        odometrys.pop_back();
        odometrys.push_back(odometry);
    }
}

void Predictor::setBulletSpeed(float v)
{
    if(0<v&&v<15.5)    //步兵弹速分档,具体根据实际情况微调,可以问电控
    {
        v = 14.5;
    }
    if(15.5<v&&v<19.5)
    {
        v = 17.5;
    }
    if(19.5<v&&v<=30)
    {
        v = 29.5;
    }

    bullet_speed = v;
}

void Predictor::fit(int type, int n)
{
    int size = odometrys.size();
    if(type == RADIAL)
    {
        int x_num = n + 1;
        //构造矩阵U和Y
        Mat mat_u(size, x_num, CV_64F);
        Mat mat_y(size, 1, CV_64F);

        for (int i = 0; i < mat_u.rows; ++i)
            for (int j = 0; j < mat_u.cols; ++j)
            {
                mat_u.at<double>(i, j) = pow(odometrys.at(i).time-odometrys.at(0).time, j);
            }

        for (int i = 0; i < mat_y.rows; ++i)
        {
            mat_y.at<double>(i, 0) = odometrys.at(i).enemy_distance;
        }

        //矩阵运算，获得系数矩阵K
        Mat mat_k(x_num, 1, CV_64F);
        //mat_k = (mat_u.t()*mat_u).inv()*mat_u.t()*mat_y;
        solve(mat_u.t()*mat_u, mat_u.t()*mat_y, mat_k);

        if(n == 1)
        {
            a_r = 0;
            v_r = mat_k.at<double>(1, 0);
        }
        if(n == 2)
        {
            a_r = 2*mat_k.at<double>(2, 0);
            v_r = mat_k.at<double>(1, 0);
        }
        return;
    }

    if(type == TANGENTIAL)
    {
        int x_num = n + 1;
        Mat mat_u(size, x_num, CV_64F);
        Mat mat_y(size, 1, CV_64F);

        for (int i = 0; i < mat_u.rows; ++i)
            for (int j = 0; j < mat_u.cols; ++j)
            {
                mat_u.at<double>(i, j) = pow(odometrys.at(i).time-odometrys.at(0).time, j);
            }


        mat_y.at<double>(0, 0) = 0;
        for (int i = 1; i < mat_y.rows; ++i)
        {
            double delta_theta;

            if(abs(odometrys.at(i).self_gimbal_yaw - odometrys.at(i-1).self_gimbal_yaw) < 180)
            {
                delta_theta = (odometrys.at(i).self_gimbal_yaw + odometrys.at(i).enemy_angle[0])
                        - (odometrys.at(i-1).self_gimbal_yaw + odometrys.at(i-1).enemy_angle[0]);
            }
            else
            {
                if(odometrys.at(i-1).self_gimbal_yaw > 270)
                {
                    delta_theta = odometrys.at(i).self_gimbal_yaw - (odometrys.at(i-1).self_gimbal_yaw - 360)
                            + (odometrys.at(i).enemy_angle[0] - odometrys.at(i-1).enemy_angle[0]);
                }
                if(odometrys.at(i).self_gimbal_yaw > 270)
                {
                    delta_theta = (odometrys.at(i).self_gimbal_yaw - 360) - odometrys.at(i-1).self_gimbal_yaw
                            + (odometrys.at(i).enemy_angle[0] - odometrys.at(i-1).enemy_angle[0]);
                }
            }
            delta_theta = delta_theta*CV_PI/180;

            mat_y.at<double>(i, 0) = mat_y.at<double>(i - 1, 0) - 0.5*(odometrys.at(i).enemy_distance + odometrys.at(i).enemy_distance)*delta_theta;
        }

        Mat mat_k(x_num, 1, CV_64F);
        //mat_k = (mat_u.t()*mat_u).inv()*mat_u.t()*mat_y;
        solve(mat_u.t()*mat_u, mat_u.t()*mat_y, mat_k);
        if(n == 1)
        {
            a_t = 0;
            v_t = -mat_k.at<double>(1, 0);
        }
        if(n == 2)
        {
            a_t = -2*mat_k.at<double>(2, 0);
            v_t = -mat_k.at<double>(1, 0);
        }
        return;
    }

    if(type == ROTATE)
    {
        int x_num = n + 1;
        Mat mat_u(size, x_num, CV_64F);
        Mat mat_y(size, 1, CV_64F);

        for (int i = 0; i < mat_u.rows; ++i)
            for (int j = 0; j < mat_u.cols; ++j)
            {
                mat_u.at<double>(i, j) = pow(odometrys.at(i).time-odometrys.at(0).time, j);
            }


        for (int i = 0; i < mat_y.rows; ++i)
        {
            mat_y.at<double>(i, 0) = 180/CV_PI*acos(odometrys.at(i).enemy_direction.x);
        }

        Mat mat_k(x_num, 1, CV_64F);
        //mat_k = (mat_u.t()*mat_u).inv()*mat_u.t()*mat_y;
        solve(mat_u.t()*mat_u, mat_u.t()*mat_y, mat_k);

        if(n == 1)
        {
            alpha = 0;
            omega = mat_k.at<double>(1, 0);
        }
        if(n == 2)
        {
            alpha = 2*mat_k.at<double>(2, 0);
            omega = mat_k.at<double>(1, 0);
        }
        return;
    }
}

void Predictor::print()
{
    std::ofstream out("/home/nuc/pose.txt", std::ios::app);
    for(int i = 0; i < odometrys.size(); i++)
    {
        odometrys.at(i).print();
    }
    /*c*/out<<a_r<<" "<<v_r<<" "<<a_t<<" "<<v_t<<" "<<alpha<<" "<<omega<<endl<<endl;
}

void Predictor::predict()
{
    if(odometrys.size() >= 3)
    {
        fit(RADIAL, 2);
        fit(TANGENTIAL, 2);
        fit(ROTATE, 1);

        if(abs(a_r) <= 10 && abs(a_r) >= 4)
        {
            move_module_r = VELOCITYACCELERATING;
        }
        else
        {
            move_module_r = VELOCITYUNCHANGE;
            fit(RADIAL, 1);
            if(abs(v_r) <= 3)
            {
                move_module_r = STAYSTILL;
            }

        }

        if(abs(a_t) <= 10 && abs(a_t) >= 4)
        {
            move_module_t = VELOCITYACCELERATING;
        }
        else
        {
            move_module_t = VELOCITYUNCHANGE;
            fit(RADIAL, 1);
            if(abs(v_t) <= 3)
            {
                move_module_t = STAYSTILL;
            }

        }
        if(odometrys.at(odometrys.size() - 1).enemy_num == 7)
        {
            if(odometrys.size() == queue_size)
            {
                move_module_t = SENTRY;
            }
        }

        if(abs(omega) > 90)
        {
            rotate_module = OMEGAUNCHANGE;
        }
        else
        {
            rotate_module = STAYSTILL;
        }


        double predict_time = 1*(double)(bullet_speed/odometrys.at(odometrys.size() - 1).enemy_distance);

        switch (move_module_r)
        {
        case STAYSTILL:
        {
            delta_pitch_predict = 0;
            break;
        }
        case VELOCITYUNCHANGE:
        {
            double d_predict = odometrys.at(odometrys.size() - 1).enemy_distance + v_r*predict_time;
            double angleNow = odometrys.at(odometrys.size() - 1).self_gimbal_pitch - odometrys.at(odometrys.size() - 1).enemy_angle[1];
            delta_pitch_predict = gravityOffset(bullet_speed, angleNow, d_predict*1000) - gravityOffset(bullet_speed, angleNow, odometrys.at(odometrys.size() - 1).enemy_distance*1000);
            break;
        }
        case VELOCITYACCELERATING:
        {
            double d_predict = odometrys.at(odometrys.size() - 1).enemy_distance + v_r*predict_time + 0.5*a_r*predict_time*predict_time;
            double angleNow = odometrys.at(odometrys.size() - 1).self_gimbal_pitch - odometrys.at(odometrys.size() - 1).enemy_angle[1];
            delta_pitch_predict = gravityOffset(bullet_speed, angleNow, d_predict*1000) - gravityOffset(bullet_speed, angleNow, odometrys.at(odometrys.size() - 1).enemy_distance*1000);
            break;
        }
        default:
        {
            delta_pitch_predict = 0;
            break;
        }
        }

        switch (move_module_t)
        {
        case STAYSTILL:
            delta_yaw_predict = 0;
            break;
        case VELOCITYUNCHANGE:
            delta_yaw_predict = v_t*predict_time/odometrys.at(odometrys.size() - 1).enemy_distance;
            break;
        case VELOCITYACCELERATING:
            delta_yaw_predict = (v_t*predict_time + 0.5*a_t*predict_time*predict_time)/odometrys.at(odometrys.size() - 1).enemy_distance;
            break;
        case SENTRY:
        {
//            if(!sentry_predict_done)
//            {
//                delta_yaw_predict = v_t*predict_time/odometrys.at(odometrys.size() - 1).enemy_distance;
//                sentry_predict_done = true;
//            }
//            if(pre_v_t * v_t < 0)
//            {
//                sentry_predict_done = false;
//                delta_yaw_predict = v_t*predict_time/odometrys.at(odometrys.size() - 1).enemy_distance;
//            }
//            pre_v_t = v_t;
            delta_yaw_predict = 0;
            if(v_t > 0.3)
            {
                if(bullet_speed > 23)
                {
                    delta_yaw_predict = 3;
                }
                else
                {
                    delta_yaw_predict = 10;
                }
            }
            if(v_t < -0.3)
            {
                if(bullet_speed > 23)
                {
                    delta_yaw_predict = -3;
                }
                else
                {
                    delta_yaw_predict = -10;
                }
            }
        }
            break;
        default:

            delta_yaw_predict = 0;
            break;
        }

        switch (rotate_module)
        {
        case STAYSTILL:
            break;
        case OMEGAUNCHANGE:
        {
            double enemy_width = 0.3;
            double armor_direction = acos(odometrys.at(odometrys.size() - 1).enemy_direction.x)*180/CV_PI;
            if(armor_direction < 70)
            {
                delta_yaw_predict = (90 - armor_direction)*CV_PI/180*enemy_width/odometrys.at(odometrys.size() - 1).enemy_distance;
            }
            if(armor_direction >= 70 && armor_direction <= 110)
            {
                delta_yaw_predict = 0;
            }
            if(armor_direction > 110)
            {
                delta_yaw_predict = (90 - armor_direction)*CV_PI/180*enemy_width/odometrys.at(odometrys.size() - 1).enemy_distance;
            }
        }
            break;
        default:
            break;
        }


        if(move_module_t != SENTRY)
        {
            delta_pitch = 0.66*delta_pitch + 0.34*delta_pitch_predict;
            delta_yaw = 0.66*delta_yaw + 0.34*delta_yaw_predict;
        }
        else
        {
            delta_pitch = delta_pitch_predict;
            delta_yaw = delta_yaw_predict;
        }


        if(delta_pitch > 1)
        {
            delta_pitch = 1;
        }
        if(delta_pitch < -1)
        {
            delta_pitch = -1;
        }
        if(delta_yaw > 15)
        {
            delta_yaw = 15;
        }
        if(delta_yaw < -15)
        {
            delta_yaw = -15;
        }
    }
    else
    {
        delta_pitch = 0;
        delta_yaw = 0;
    }

}


}
